package simulator.meta.agent;

import javax.vecmath.Vector3d;

import simbad.sim.Agent;

public abstract class ControlledAgent extends Agent{
	//Constants:
	protected final int STEP_INCREASE = 1;
	protected final int TURN_INCREASE = 50;
	protected final boolean GOING_FORTH = true;
	protected final boolean TURNNING_RIGHT = true;
	
	//Attributes:
	protected int step;
	protected int turn;
	protected boolean stepStatus;
	protected boolean turnningStatus;
	
	//Constructor:
	public ControlledAgent(Vector3d arg0, String arg1) {
		super(arg0, arg1);
		this.stepStatus = this.GOING_FORTH;
	}
	
	//Methods:
	public void stepUp(){
		if(this.stepStatus!=this.GOING_FORTH&&this.step==(-1*this.STEP_INCREASE)){
			this.step=0;
			this.stepStatus = this.GOING_FORTH;
		}else{
			this.step+=(this.step>3)?0:this.STEP_INCREASE;
		}
	}
	public void stepDown(){
		if(this.stepStatus==this.GOING_FORTH&&this.step==this.STEP_INCREASE){
			this.step=0;
			this.stepStatus = !this.GOING_FORTH;
		}else{
			this.step-=(this.step<-3)?0:this.STEP_INCREASE;
		}
	}
	public void turnLeft(){
		if(this.turnningStatus==this.TURNNING_RIGHT&&this.turn==(-1*this.TURN_INCREASE)){
			this.turn=0;
			this.turnningStatus = !this.TURNNING_RIGHT;
		}else{
			this.turn+=(this.step>180)?0:this.TURN_INCREASE;
		}
	}
	public void turnRight(){
		if(this.turnningStatus!=this.TURNNING_RIGHT&&this.turn==this.TURN_INCREASE){
			this.turn=0;
			this.turnningStatus = this.TURNNING_RIGHT;
		}else{
			this.turn-=(this.step<-180)?0:this.TURN_INCREASE;
		}
	}
	
//	/**
//	 * @return azimuth.
//	 */
//	public double getAzimuth(){
//		return this.azimuth;
//	}
//	/**
//	 * This method allows to know current agent azimuth.
//	 * @param currentX: Current x.
//	 * @param currentZ: Current z.
//	 * @return azimuth.
//	 */
//	public double getAzimuth(double currentX, double currentZ){
//		double azimut;
//		SourceGetter.out.debug("ControlledAgent> [getAzimuth("+currentX+", "+currentZ+")]", "last values = ("+this.lastX+","+this.lastZ+")");
//		if(this.stepStatus == this.GOING_FORTH){
//			azimut = this.getAzimuth(this.lastX, this.lastZ, currentX, currentZ);
//			SourceGetter.out.debug("ControlledAgent> [getAzimuth("+currentX+", "+currentZ+")]", "GOING_FORTH = "+String.valueOf(azimut));
//		}else{
//			azimut = this.getAzimuth(currentX, currentZ, this.lastX, this.lastZ);
//			SourceGetter.out.debug("ControlledAgent> [getAzimuth("+currentX+", "+currentZ+")]", "GOING_BACK = "+String.valueOf(azimut));
//		}
//		if(currentX != this.lastX && currentZ != this.lastZ){
//			this.lastX = currentX;
//			this.lastZ = currentZ;
//			SourceGetter.out.debug("ControlledAgent> [getAzimuth("+currentX+", "+currentZ+")]", "Updating last values.");
//		}
//		return azimut;
//	}
//	
//	/**
//	 * This method allows to know current agent azimuth.
//	 * @param x1: Last x.
//	 * @param z1: Last z.
//	 * @param x2: Current x.
//	 * @param z2: Current z.
//	 * @return azimuth.
//	 */
//	public double getAzimuth(double x1, double z1, double x2, double z2){
//		double F = ((180/Math.PI)*Math.acos((x2-x1)/(Math.sqrt(Math.pow(x1-x2, 2)+Math.pow(z1-z2, 2)))));
//		return 360-(((z2-z1)<0)?(360-F):F);
//	}
}
